// Include files.
#include "FpkAssert.hpp"
#include "FpkException.hpp"
#include "FourMotorTankDriveSubsystem.hpp"

using namespace std;

/**
 *  A null CANJaguar reference.
 */
CANJaguar &FourMotorTankDriveSubsystem::s_nullCANJaguar(*(CANJaguar*)(NULL));

/**
 *  Constructor for FourMotorTankDriveSubsystem.
 */
FourMotorTankDriveSubsystem::FourMotorTankDriveSubsystem
(
	UINT32 leftFrontMotorChannel, 
	UINT32 leftBackMotorChannel,
	UINT32 rightFrontMotorChannel, 
	UINT32 rightBackMotorChannel
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesCanJaguars(false)
	, m_cjLeftFront(s_nullCANJaguar)
	, m_cjLeftBack(s_nullCANJaguar)
	, m_cjRightFront(s_nullCANJaguar)
	, m_cjRightBack(s_nullCANJaguar)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotorChannel, 
			leftBackMotorChannel, 
			rightFrontMotorChannel, 
			rightBackMotorChannel
		)
	)
{
	Startup();
}

/**
 *  Constructor for FourMotorTankDriveSubsystem.
 */
FourMotorTankDriveSubsystem::FourMotorTankDriveSubsystem
(
	CANJaguar *leftFrontMotor, 
	CANJaguar *leftBackMotor,
	CANJaguar *rightFrontMotor, 
	CANJaguar *rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesCanJaguars(true)
	, m_cjLeftFront(*leftFrontMotor)
	, m_cjLeftBack(*leftBackMotor)
	, m_cjRightFront(*rightFrontMotor)
	, m_cjRightBack(*rightBackMotor)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
{
	Startup();
}

/**
 *  Constructor for FourMotorTankDriveSubsystem.
 */
FourMotorTankDriveSubsystem::FourMotorTankDriveSubsystem
(
	CANJaguar &leftFrontMotor, 
	CANJaguar &leftBackMotor,
	CANJaguar &rightFrontMotor, 
	CANJaguar &rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesCanJaguars(true)
	, m_cjLeftFront(leftFrontMotor)
	, m_cjLeftBack(leftBackMotor)
	, m_cjRightFront(rightFrontMotor)
	, m_cjRightBack(rightBackMotor)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
{
	Startup();
}

/**
 *  Constructor for FourMotorTankDriveSubsystem.
 */
FourMotorTankDriveSubsystem::FourMotorTankDriveSubsystem
(
	SpeedController *leftFrontMotor, 
	SpeedController *leftBackMotor,
	SpeedController *rightFrontMotor, 
	SpeedController *rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesCanJaguars(false)
	, m_cjLeftFront(s_nullCANJaguar)
	, m_cjLeftBack(s_nullCANJaguar)
	, m_cjRightFront(s_nullCANJaguar)
	, m_cjRightBack(s_nullCANJaguar)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
{
	Startup();
}

/**
 *  Constructor for FourMotorTankDriveSubsystem.
 */
FourMotorTankDriveSubsystem::FourMotorTankDriveSubsystem
(
	SpeedController &leftFrontMotor, 
	SpeedController &leftBackMotor,
	SpeedController &rightFrontMotor, 
	SpeedController &rightBackMotor
) :
	m_reversed(false)
	, m_ownsDrive(true)
	, m_usesCanJaguars(false)
	, m_cjLeftFront(s_nullCANJaguar)
	, m_cjLeftBack(s_nullCANJaguar)
	, m_cjRightFront(s_nullCANJaguar)
	, m_cjRightBack(s_nullCANJaguar)
	, m_drive
	(
		*new RobotDrive
		(
			leftFrontMotor, 
			leftBackMotor, 
			rightFrontMotor, 
			rightBackMotor
		)
	)
{
	Startup();
}

FourMotorTankDriveSubsystem::~FourMotorTankDriveSubsystem()
{
}

/**
 *  Drive the specified amount tank style.
 *  
 *  This function is untested.
 *  
 *  @param leftValue The amount to drive the left drive motors.
 *  @prarm rightValue The amount to drive the right drive motors.
 */
void FourMotorTankDriveSubsystem::DriveByTank(double leftValue, double rightValue)
{
	m_drive.TankDrive(leftValue, rightValue);
	m_safetyHelper->Feed();
	m_driveUpdated = true;
}

/**
 *  Drive the specified magnitude and curve amount.
 *  
 *  This function is untested.
 *  
 *  @param magnitude The magnitude to drive.
 *  @prarm curve the amount to curve.
 */
void FourMotorTankDriveSubsystem::DriveByMagnitudeAndCurve(double magnitude, double curve)
{
	m_drive.Drive(magnitude, curve);
	m_driveUpdated = true;
	m_safetyHelper->Feed();
}

/**
 *  Sets the ControlMode.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 *  
 *  This function seems to work to the extent that it doesn't crash if used properly.
 *  
 *  @param controlMode The ControlMode to switch to.
 */
void FourMotorTankDriveSubsystem::SetControlMode(ControlMode controlMode)
{
	if (m_usesCanJaguars)
	{
		CANJaguar::ControlMode cjControlMode = CANJaguar::ControlMode(controlMode);
		
		m_cjLeftFront.ChangeControlMode(cjControlMode);
		m_cjLeftBack.ChangeControlMode(cjControlMode);
		m_cjRightFront.ChangeControlMode(cjControlMode);
		m_cjRightBack.ChangeControlMode(cjControlMode);
		
		if (m_pidValues.count(controlMode) > 0)
		{
			PidValue leftFront = m_pidValues[controlMode][MotorPositionLeftFront];
			PidValue leftBack = m_pidValues[controlMode][MotorPositionLeftBack];
			PidValue rightFront = m_pidValues[controlMode][MotorPositionRightFront];
			PidValue rightBack = m_pidValues[controlMode][MotorPositionRightBack];
			
			m_cjLeftFront.SetPID(leftFront.m_p, leftFront.m_i, leftFront.m_d);
			m_cjLeftBack.SetPID(leftBack.m_p, leftBack.m_i, leftBack.m_d);
			m_cjRightFront.SetPID(rightFront.m_p, rightFront.m_i, rightFront.m_d);
			m_cjRightBack.SetPID(rightBack.m_p, rightBack.m_i, rightBack.m_d);
			
			//printf("REMOVE THIS AFTER!  PID: %f %f %f\n", leftFront.m_p, leftFront.m_i, leftFront.m_d);
		}
		
		m_cjLeftFront.EnableControl(0);
		m_cjLeftBack.EnableControl(0);
		m_cjRightFront.EnableControl(0);
		m_cjRightBack.EnableControl(0);
		
		m_controlMode = controlMode;
	}
	else
	{
		throw FpkException("Not CANJaguars!");
	}
}

/**
 *  Gets the current ControlMode.
 *  
 *  Currently only applicable if CANJaguars are used.
 *  If CANJaguars aren't being used, ControlModePercentVBus is returned.
 *  
 *  This function is untested.
 */
DriveSubsystem::ControlMode FourMotorTankDriveSubsystem::GetControlMode()
{
	return m_controlMode;
}

/**
 *  Sets the SpeedReference.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 *  
 *  This function seems to work to the extent that it doesn't crash if used properly.
 *  
 *  @param speedReference The SpeedReference to switch to.
 */
void FourMotorTankDriveSubsystem::SetSpeedReference(SpeedReference speedReference)
{
	if (m_usesCanJaguars)
	{
		CANJaguar::SpeedReference cjSpeedReference = CANJaguar::SpeedReference(speedReference);
		
		m_cjLeftFront.SetSpeedReference(cjSpeedReference);
		m_cjLeftBack.SetSpeedReference(cjSpeedReference);
		m_cjRightFront.SetSpeedReference(cjSpeedReference);
		m_cjRightBack.SetSpeedReference(cjSpeedReference);
	}
	else
	{
		throw FpkException("Not CANJaguars!");
	}
}

/**
 *  Sets the DistanceReference.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 *  
 *  This function seems to work to the extent that it doesn't crash if used properly.
 *  
 *  @param distanceReference The DistanceReference to switch to.
 */
void FourMotorTankDriveSubsystem::SetDistanceReference(DistanceReference distanceReference)
{
	if (m_usesCanJaguars)
	{
		CANJaguar::PositionReference cjPositionReference = CANJaguar::PositionReference(distanceReference);
		
		m_cjLeftFront.SetPositionReference(cjPositionReference);
		m_cjLeftBack.SetPositionReference(cjPositionReference);
		m_cjRightFront.SetPositionReference(cjPositionReference);
		m_cjRightBack.SetPositionReference(cjPositionReference);
	}
	else
	{
		throw FpkException("Not CANJaguars!");
	}
}

/**
 *  Sets the NeutralMode.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 *  
 *  This function is untested.
 *  
 *  @param neutralMode The NeutralMode to switch to.
 */
void FourMotorTankDriveSubsystem::SetNeutralMode(NeutralMode neutralMode)
{
	if (m_usesCanJaguars)
	{
		CANJaguar::NeutralMode cjNeutralMode = CANJaguar::NeutralMode(neutralMode);
		
		m_cjLeftFront.ConfigNeutralMode(cjNeutralMode);
		m_cjLeftBack.ConfigNeutralMode(cjNeutralMode);
		m_cjRightFront.ConfigNeutralMode(cjNeutralMode);
		m_cjRightBack.ConfigNeutralMode(cjNeutralMode);
	}
	else
	{
		throw FpkException("Not CANJaguars!");
	}
}

/**
 *  Sets the motor invertations.
 *  
 *  This function is untested.
 *  
 *  @param leftFrontIsInverted The desired invertation of the left front motor.
 *  @param leftBackIsInverted The desired invertation of the left back motor.
 *  @param rightFrontIsInverted The desired invertation of the right front motor.
 *  @param rightBackIsInverted The desired invertation of the right back motor.
 */
void FourMotorTankDriveSubsystem::SetMotorInvertations
(
	bool leftFrontIsInverted, 
	bool leftBackIsInverted, 
	bool rightFrontIsInverted, 
	bool rightBackIsInverted
)
{
	m_motorInvertationLeftFront = leftFrontIsInverted;
	m_motorInvertationLeftBack = leftBackIsInverted;
	m_motorInvertationRightFront = rightFrontIsInverted;
	m_motorInvertationRightBack = rightBackIsInverted;
	
	m_drive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, m_motorInvertationLeftFront);
	m_drive.SetInvertedMotor(RobotDrive::kRearLeftMotor, m_motorInvertationLeftBack);
	m_drive.SetInvertedMotor(RobotDrive::kFrontRightMotor, m_motorInvertationRightFront);
	m_drive.SetInvertedMotor(RobotDrive::kRearRightMotor, m_motorInvertationRightBack);
}


/**
 *  Sets the motor invertation of the motor at the specified MotorPosition.
 *  
 *  This function seems to work.
 *  
 *  @param motorPosition The specified MotorPosition of the motor to be inverted or uninverted.
 *  @param isInverted The desired invertation of the specified motor.
 */
void FourMotorTankDriveSubsystem::SetMotorInvertation(MotorPosition motorPosition, bool isInverted)
{
	switch (motorPosition)
	{
		case MotorPositionLeftFront:
			m_motorInvertationLeftFront = isInverted;
			break;
		case MotorPositionLeftBack:
			m_motorInvertationLeftBack = isInverted;
			break;
		case MotorPositionRightFront:
			m_motorInvertationRightFront = isInverted;
			break;
		case MotorPositionRightBack:
			m_motorInvertationRightBack = isInverted;
			break;
	}
	
	m_drive.SetInvertedMotor(RobotDrive::MotorType(motorPosition), isInverted);
}

/**
 *  Sets the encoder codes per revolution.
 *  
 *  This function is untested.
 *  
 *  @param codePerRevolution The specified encoder codes per revolution of all of the drive motors.
 */
void FourMotorTankDriveSubsystem::SetEncodersCodesPerRevolution(UINT16 codePerRevolution)
{
	SetEncodersCodesPerRevolution
	(
		codePerRevolution, 
		codePerRevolution, 
		codePerRevolution, 
		codePerRevolution
	);
}

/**
 *  Sets the encoder codes per revolution on each of the motors.
 *  
 *  This function is untested.
 *  
 *  @param leftFrontCodesPerRevolution The specified encoder codes per revolution of the left front motor.
 *  @param leftBackCodesPerRevolution The specified encoder codes per revolution of the left back motor.
 *  @param rightFrontCodesPerRevolution The specified encoder codes per revolution of the right front motor.
 *  @param rightBackCodesPerRevolution The specified encoder codes per revolution of the right back motor.
 */
void FourMotorTankDriveSubsystem::SetEncodersCodesPerRevolution
(
	UINT16 leftFrontCodesPerRevolution, 
	UINT16 leftBackCodesPerRevolution, 
	UINT16 rightFrontCodesPerRevolution, 
	UINT16 rightBackCodesPerRevolution
)
{
	m_cjLeftFront.ConfigEncoderCodesPerRev(leftFrontCodesPerRevolution);
	m_cjLeftBack.ConfigEncoderCodesPerRev(leftBackCodesPerRevolution);
	m_cjRightFront.ConfigEncoderCodesPerRev(rightFrontCodesPerRevolution);
	m_cjRightBack.ConfigEncoderCodesPerRev(rightBackCodesPerRevolution);
}

/**
 *  Sets the encoder codes per revolution on the specified motor.
 *  
 *  This function seems to work.
 *  
 *  @param motorPosition The specified MotorPosition of the motor.
 *  @param codesPerRevolution The specified encoder codes per revolution of the specified motor.
 */
void FourMotorTankDriveSubsystem::SetEncoderCodesPerRevolution
(
	MotorPosition motorPosition, 
	UINT16 codesPerRevolution
)
{
	if (m_usesCanJaguars)
	{
		switch (motorPosition)
		{
			case MotorPositionLeftFront:
				m_cjLeftFront.ConfigEncoderCodesPerRev(codesPerRevolution);
				break;
			case MotorPositionLeftBack:
				m_cjLeftBack.ConfigEncoderCodesPerRev(codesPerRevolution);
				break;
			case MotorPositionRightFront:
				m_cjRightFront.ConfigEncoderCodesPerRev(codesPerRevolution);
				break;
			case MotorPositionRightBack:
				m_cjRightBack.ConfigEncoderCodesPerRev(codesPerRevolution);
				break;
		}
	}
	else
	{
		throw FpkException("Not CANJaguars!");
	}
}

/**
 *  Sets the PID values of each of the motors when in speed control mode.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 *  
 *  This function is untested.
 *  
 *  @param p The desired Proportional gain.
 *  @param i The desired Intregral gain.
 *  @param d The desired Differential gain. 
 */
void FourMotorTankDriveSubsystem::SetPIDs
(
	ControlMode controlMode,
	double p, 
	double i, 
	double d
)
{
	if (m_usesCanJaguars)
	{
		m_pidValues[controlMode][MotorPositionLeftFront] = PidValue(p, i, d);
		m_pidValues[controlMode][MotorPositionLeftBack] = PidValue(p, i, d);
		m_pidValues[controlMode][MotorPositionRightFront] = PidValue(p, i, d);
		m_pidValues[controlMode][MotorPositionRightBack] = PidValue(p, i, d);
		
		if (m_controlMode == controlMode)
		{
			m_cjLeftFront.SetPID(p, i, d);
			m_cjLeftBack.SetPID(p, i, d);
			m_cjRightFront.SetPID(p, i, d);
			m_cjRightBack.SetPID(p, i, d);
			
			printf("REMOVE THIS AFTER!  PID: %f %f %f\n", p, i, d);
		}
	}
	else
	{
		throw FpkException("Not CANJaguars!");
	}
}

/**
 *  Sets the PID values of the specified drive motor when in speed control mode.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 *  
 *  This function seems to work.
 *  
 *  @param motorPosition The specified MotorPosition.
 *  @param p The desired Proportional gain.
 *  @param i The desired Intregral gain.
 *  @param d The desired Differential gain. 
 */
void FourMotorTankDriveSubsystem::SetPID
(
	ControlMode controlMode,
	MotorPosition motorPosition, 
	double p,
	double i,
	double d
)
{
	if (m_usesCanJaguars)
	{
		m_pidValues[controlMode][motorPosition] = PidValue(p, i, d);
		
		if (m_controlMode == controlMode)
		{
			switch (motorPosition)
			{
				case MotorPositionLeftFront:
					m_cjLeftFront.SetPID(p, i, d);
					break;
				case MotorPositionLeftBack:
					m_cjLeftBack.SetPID(p, i, d);
					break;
				case MotorPositionRightFront:
					m_cjRightFront.SetPID(p, i, d);
					break;
				case MotorPositionRightBack:
					m_cjRightBack.SetPID(p, i, d);
					break;
			}
			
			printf("REMOVE THIS AFTER!  PID: %f %f %f\n", p, i, d);
		}
	}
	else
	{
		throw FpkException("Not CANJaguars!");
	}
}

/**
 *  Sets the maximum output of the drive system.
 *  
 *  This function seems to work.
 *  
 *  @param maximumOutput The desired maximum output.
 */
void FourMotorTankDriveSubsystem::SetMaximumOutput(double maximumOutput)
{
	m_maxOutput = maximumOutput;
	
	m_drive.SetMaxOutput(maximumOutput);
}

double FourMotorTankDriveSubsystem::GetDistanceThreshold()
{
	return m_distanceThreshold;
}

void FourMotorTankDriveSubsystem::SetDistanceThreshold(double distanceThreshold)
{
	m_distanceThreshold = distanceThreshold;
}

double FourMotorTankDriveSubsystem::GetExpiration()
{
	return m_drive.GetExpiration();
}

void FourMotorTankDriveSubsystem::SetExpiration(double expiration)
{
	m_drive.SetExpiration(expiration);
}

/**
 *  Stops the drive system.
 *  
 *  This function is untested.
 */
void FourMotorTankDriveSubsystem::Stop()
{
	m_drive.StopMotor();
}

/**
 *  Prints the faults of the CANJaguars.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, functions will be called on null.
 * 
 *  This function is untested.
 */
void FourMotorTankDriveSubsystem::PrintFaults(void)
{
	printf
	(
		"Drive motors faults: %d, %d, %d, %d\n",
		m_cjLeftFront.GetFaults(),
		m_cjLeftBack.GetFaults(),
		m_cjRightFront.GetFaults(),
		m_cjRightBack.GetFaults()
	);
}

void FourMotorTankDriveSubsystem::Enable()
{
	if(m_usesCanJaguars)
	{
		m_cjLeftFront.EnableControl();
		m_cjLeftBack.EnableControl();
		m_cjRightFront.EnableControl();
		m_cjRightBack.EnableControl();
	}
}

void FourMotorTankDriveSubsystem::Periodic()
{
	if (!m_driveUpdated)
	{
		m_drive.Drive(0, 0);
		m_safetyHelper->Feed();
		
		//printf("REMOVE THIS AFTER!  Drive: Not Updating\n");
	}
	else
		m_driveUpdated = false;
	
	if (m_usesCanJaguars)
	{
		m_counterPeriods++;
		
		if (m_counterPeriods >= 10)
		{
			switch (m_motorCanidateForCheck)
			{
				case MotorPositionLeftFront:
					if ((m_controlMode == ControlModeSpeed) && (m_cjLeftFront.Get() != 0.0) && (m_cjLeftFront.GetOutputVoltage() == 0.0))
						m_cjLeftFront.EnableControl();
					if (m_cjLeftFront.GetFaults())
						PrintFaults();
					m_motorCanidateForCheck = MotorPositionRightFront;
					break;
				case MotorPositionRightFront:
					if ((m_controlMode == ControlModeSpeed) && (m_cjRightFront.Get() != 0.0) && (m_cjRightFront.GetOutputVoltage() == 0.0))
						m_cjRightFront.EnableControl();
					if (m_cjRightFront.GetFaults())
						PrintFaults();
					m_motorCanidateForCheck = MotorPositionLeftBack;
					break;
				case MotorPositionLeftBack:
					if ((m_controlMode == ControlModeSpeed) && (m_cjLeftBack.Get() != 0.0) && (m_cjLeftBack.GetOutputVoltage() == 0.0))
						m_cjLeftBack.EnableControl();
					if (m_cjLeftBack.GetFaults())
						PrintFaults();
					m_motorCanidateForCheck = MotorPositionRightBack;
					break;
				case MotorPositionRightBack:
					if ((m_controlMode == ControlModeSpeed) && (m_cjRightBack.Get() != 0.0) && (m_cjRightBack.GetOutputVoltage() == 0.0))
						m_cjRightBack.EnableControl();
					if (m_cjRightBack.GetFaults())
						PrintFaults();
					m_motorCanidateForCheck = MotorPositionLeftFront;
					break;
				default:
					printf("Drive: Invalid Motor Canidate For Check\n");
					m_motorCanidateForCheck = MotorPositionLeftFront;
					break;
			}
			
			m_counterPeriods = 0;
		}
	}
}

void FourMotorTankDriveSubsystem::Disable()
{
	Stop();
}

FourMotorTankDriveSubsystem::PidValue::PidValue() :
	m_p(0),
	m_i(0),
	m_d(0)
{
}

FourMotorTankDriveSubsystem::PidValue::PidValue(double p, double i, double d) :
	m_p(p),
	m_i(i),
	m_d(d)
{
}

void FourMotorTankDriveSubsystem::Startup()
{
	m_motorInvertationLeftFront = false;
	m_motorInvertationLeftBack = false;
	m_motorInvertationRightFront = true;
	m_motorInvertationRightBack = true;
	
	m_driveUpdated = false;
	
	m_maxOutput = 1.0;
	
	m_distanceThreshold = 0.5;
	
	m_counterPeriods = 0;
	
	m_controlMode = ControlModePercentVBus;
	
	m_drive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, m_motorInvertationLeftFront);
	m_drive.SetInvertedMotor(RobotDrive::kRearLeftMotor, m_motorInvertationLeftBack);
	m_drive.SetInvertedMotor(RobotDrive::kFrontRightMotor, m_motorInvertationRightFront);
	m_drive.SetInvertedMotor(RobotDrive::kRearRightMotor, m_motorInvertationRightBack);
	
	m_drive.SetMaxOutput(m_maxOutput);
	
	SetControlMode(m_controlMode);
	
	if (m_usesCanJaguars)
		m_motorCanidateForCheck = MotorPositionLeftFront;
	
	m_safetyHelper = new MotorSafetyHelper(&m_drive);
	m_safetyHelper->SetSafetyEnabled(true);
	m_drive.SetSafetyEnabled(false);
}

double FourMotorTankDriveSubsystem::GetMotorInvertationMultiplier(bool isInverted)
{
	if (isInverted)
		return -1;
	else
		return 1;
}

